#include "formation_avoidance/formation.h"
#include "trajectory_formation/backward.hpp"


namespace backward {
backward::SignalHandling sh;
}

using namespace std;
using namespace Eigen;





int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "agent0");
    ros::NodeHandle nh("~");

    const int LOOPRATE = 40;
    ros::Rate loop_rate(LOOPRATE);  

	int AGENT_ID=0;
    nh.getParam("agent_id",AGENT_ID);
	int agent_num;
	nh.getParam("/AGENT_NUM",agent_num);

    ROS_INFO("agent %d node start-----",AGENT_ID);

    Eigen::Vector3f start_p(2.8,0,1.4);
    Eigen::Vector3f end_p(7.0,0.0,1.8);

	int HeadAngleKnotVectorSize;
	nh.getParam("/HeadAngleKnotVectorSize",HeadAngleKnotVectorSize);

    Formation_Agent formation_agent(AGENT_ID,agent_num,HeadAngleKnotVectorSize);

	double FSMFreq;
	double FSMFreq_NoExec;

	nh.getParam("/FSMFreq",FSMFreq);
	nh.getParam("/FSMFreq_NoExec",FSMFreq_NoExec);




    nh.getParam("/replan_length",formation_agent.replan_length);
    nh.getParam("/local_length",formation_agent.local_length);
    nh.getParam("/local_plan_end_length",formation_agent.local_plan_end_length);


    nh.getParam("other_id1",formation_agent.other_id1);
    nh.getParam("other_id2",formation_agent.other_id2);
    nh.getParam("other_id3",formation_agent.other_id3);

	ROS_INFO("other id : %d %d %d",formation_agent.other_id1,formation_agent.other_id2,formation_agent.other_id3);

    nh.getParam("/v",formation_agent.v);
	nh.getParam("/max_acc",formation_agent.max_acc);

    nh.getParam("/execflag",formation_agent.execflag);
	nh.getParam("/execflag",formation_agent.PathExchanger.execflag);



	formation_agent.FSMFreq=formation_agent.execflag<0.5 ? FSMFreq_NoExec: FSMFreq;

    nh.getParam("/MapUpdateFreq",formation_agent.MapUpdateFreq);


    nh.getParam("/OPTIMIZE_SAFETY_DISTANCE",formation_agent.OPTIMIZE_SAFETY_DISTANCE);
    nh.getParam("/FORMATION_TYPE",formation_agent.formation_trajectory_optimizer.FORMATION_TYPE);

    nh.getParam("/formation_edge_ratio",formation_agent.formation_trajectory_optimizer.formation_edge_ratio);
    nh.getParam("/formation_safety_distance",formation_agent.formation_trajectory_optimizer.formation_safety_distance);
    nh.getParam("/formation_base_edge_length",formation_agent.formation_trajectory_optimizer.formation_base_edge_length);

	nh.getParam("/K_IG",formation_agent.K_IG);
	nh.getParam("/K_CG",formation_agent.K_CG);
	nh.getParam("/K_LAST",formation_agent.K_LAST);
	nh.getParam("/K_V",formation_agent.K_V);

	nh.getParam("/PlanningHeadFlag",formation_agent.PlanningHeadFlag);


    formation_agent.init(nh);

	nh.getParam("x",formation_agent.initial_offset_x);
	nh.getParam("y",formation_agent.initial_offset_y);
	nh.getParam("z",formation_agent.initial_offset_z);

	ROS_INFO("id:%d, offset: %f %f %f",AGENT_ID,formation_agent.initial_offset_x,formation_agent.initial_offset_y,formation_agent.initial_offset_z);


    nh.getParam("/SAFETY_DISTANCE",formation_agent.SAFETY_DISTANCE);


    nh.getParam("/global_goal_x",formation_agent.global_goal(0));
    nh.getParam("/global_goal_y",formation_agent.global_goal(1));
    nh.getParam("/global_goal_z",formation_agent.global_goal(2));
	// formation_agent.global_goal(0)+=formation_agent.initial_offset_x;
	// formation_agent.global_goal(1)+=formation_agent.initial_offset_y;
	// formation_agent.global_goal(2)+=formation_agent.initial_offset_z;






	ROS_INFO("formation_agent.execflag: %d",formation_agent.execflag);


	ROS_INFO("/SAFETY_DISTANCE: %f",formation_agent.SAFETY_DISTANCE);

    nh.getParam("/AvoidanceFlightHeight",formation_agent.AvoidanceFlightHeight);

    // nh.getParam("/lamda_Smoothness",formation_agent.formation_trajectory_optimizer.lamda_Smoothness);
    // nh.getParam("/lamda_Distance",formation_agent.formation_trajectory_optimizer.lamda_Distance);
    // nh.getParam("/lamda_Feasibility",formation_agent.formation_trajectory_optimizer.lamda_Feasibility);
    // nh.getParam("/lamda_Formation",formation_agent.formation_trajectory_optimizer.lamda_Formation);
    // nh.getParam("/lamda_HeightLimitation",formation_agent.formation_trajectory_optimizer.lamda_HeightLimitation);
    // nh.getParam("/lamda_EndPoint",formation_agent.formation_trajectory_optimizer.lamda_EndPoint);
    // nh.getParam("/lamda_velocityDirectionCost",formation_agent.formation_trajectory_optimizer.lamda_velocityDirectionCost);

	// ROS_INFO("/lamda_velocityDirectionCost: %f",formation_agent.formation_trajectory_optimizer.lamda_velocityDirectionCost);

    nh.getParam("/plan_end_v",formation_agent.formation_trajectory_optimizer.plan_end_v);


    nh.getParam("/PlanUsingTime",formation_agent.PlanUsingTime);

    nh.getParam("/PlanningLeftTime",formation_agent.PlanningLeftTime);

	formation_agent.PlanUsingCurrentState=formation_agent.execflag<0.5 ? true  : false;


	ros::Duration(3.0).sleep();


    ros::spin();
	return 0;

}
